#include "fonctions.h"

int write_in_queue(RT_QUEUE *msgQueue, void * data, int size);

void envoyer(void * arg) {
    DMessage *msg;
    int err;

    while (1) {
        rt_printf("tenvoyer : Attente d'un message\n");
        if ((err = rt_queue_read(&queueMsgGUI, &msg, sizeof (DMessage), TM_INFINITE)) >= 0) {
            rt_printf("tenvoyer : envoi d'un message au moniteur\n");
            serveur->send(serveur, msg);
            msg->free(msg);
        } else {
            rt_printf("Error msg queue write: %s\n", strerror(-err));
        }
    }
}

void gestion_echec_comm(int status) {
    int nbEchec;
    DMessage *message;

    if (status != STATUS_OK) {
        rt_mutex_acquire(&mutexEchecComm, TM_INFINITE);
        nbEchecComm++;
        nbEchec = nbEchecComm;
        rt_mutex_release(&mutexEchecComm);

        if (nbEchec >= 3) {
            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            etatCommRobot = status;
            rt_mutex_release(&mutexEtat);

            message = d_new_message();
            message->put_state(message, status);

            rt_printf("tmove : Envoi message\n");
            if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                message->free(message);
            }
        }
    } else {
        rt_mutex_acquire(&mutexEchecComm, TM_INFINITE);
        nbEchecComm = 0;
        rt_mutex_release(&mutexEchecComm);

    }
}

void connecter(void * arg) {
    int status;
    DMessage *message;

    rt_printf("tconnect : Debut de l'exécution de tconnect\n");

    while (1) {
        rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
        status = robot->open_device(robot);
        rt_mutex_release(&mutexRobotComm);

        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;
        rt_mutex_release(&mutexEtat);

        if (status == STATUS_OK) {
            rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
            status = robot->start_insecurely(robot);
            rt_mutex_release(&mutexRobotComm);
            if (status == STATUS_OK) {
                rt_printf("tconnect : Robot démarrer\n");
            }
        }

        message = d_new_message();
        message->put_state(message, status);

        rt_printf("tconnecter : Envoi message\n");
        message->print(message, 100);

        if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
            message->free(message);
        }
    }
}

void communiquer(void *arg) {
    DMessage *msg = d_new_message();
    int var1;
    int num_msg;
    int status;
    while (threadCommuniquer == 1) {
        var1 = 1;
        num_msg = 0;
        status = 1;

        rt_printf("tserver : Début de l'exécution de serveur\n");
        serveur->open(serveur, "8000");
        rt_printf("tserver : Connexion\n");

        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommMoniteur = 0;
        rt_mutex_release(&mutexEtat);

        while (var1 > 0) {
            rt_printf("tserver : Attente d'un message\n");
            var1 = serveur->receive(serveur, msg);
            num_msg++;
            if (var1 > 0) {
                switch (msg->get_type(msg)) {
                    case MESSAGE_TYPE_ACTION:
                        rt_printf("tserver : Le message %d reçu est une action\n",
                                num_msg);
                        DAction *action = d_new_action();
                        action->from_message(action, msg);
                        switch (action->get_order(action)) {
                            case ACTION_CONNECT_ROBOT:
                                rt_printf("tserver : Action connecter robot\n");
                                rt_sem_v(&semConnecterRobot);
                                break;
                        }
                        break;
                    case MESSAGE_TYPE_MOVEMENT:
                        rt_printf("tserver : Le message reçu %d est un mouvement\n",
                                num_msg);
                        rt_mutex_acquire(&mutexMove, TM_INFINITE);
                        move->from_message(move, msg);
                        move->print(move);
                        rt_mutex_release(&mutexMove);
                        break;
                }
            }
        }
        if (var1 <= 0) {
            rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
            status = robot->stop(robot);
            rt_mutex_release(&mutexRobotComm);

            gestion_echec_comm(status);

            rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
            status = robot->close_com(robot);
            rt_mutex_release(&mutexRobotComm);

            gestion_echec_comm(status);

            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            etatCommRobot = 1;
            etatCommMoniteur = 1;
            rt_mutex_release(&mutexEtat);
            //stopper les autres threads et le mettre en attente
            serveur->close(serveur);
        }
    }
}

void deplacer(void *arg) {
    int status = 1;
    int gauche;
    int droite;

    rt_printf("tmove : Debut de l'éxecution de periodique à 1s\n");
    rt_task_set_periodic(NULL, TM_NOW, 1000000000);

    while (1) {
        /* Attente de l'activation périodique */
        rt_task_wait_period(NULL);
        rt_printf("tmove : Activation périodique\n");

        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);

        if (status == STATUS_OK) {
            rt_mutex_acquire(&mutexMove, TM_INFINITE);
            switch (move->get_direction(move)) {
                case DIRECTION_FORWARD:
                    gauche = MOTEUR_ARRIERE_LENT;
                    droite = MOTEUR_ARRIERE_LENT;
                    break;
                case DIRECTION_LEFT:
                    gauche = MOTEUR_ARRIERE_LENT;
                    droite = MOTEUR_AVANT_LENT;
                    break;
                case DIRECTION_RIGHT:
                    gauche = MOTEUR_AVANT_LENT;
                    droite = MOTEUR_ARRIERE_LENT;
                    break;
                case DIRECTION_STOP:
                    gauche = MOTEUR_STOP;
                    droite = MOTEUR_STOP;
                    break;
                case DIRECTION_STRAIGHT:
                    gauche = MOTEUR_AVANT_LENT;
                    droite = MOTEUR_AVANT_LENT;
                    break;
            }
            rt_mutex_release(&mutexMove);

            rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
            status = robot->set_motors(robot, gauche, droite);
            rt_mutex_release(&mutexRobotComm);

            gestion_echec_comm(status);
        }
    }
}
void batterie(void * arg){
    int status = 1;
    int level = -1;
    DMessage *message;
    DBattery * battery;
    battery = d_new_battery();
    
    rt_printf("tbattery : Debut de l'éxecution de periodique à 250ms\n");
    rt_task_set_periodic(NULL, TM_NOW, 250000000);
    
    while (1) 
    {
        rt_task_wait_period(NULL);
        rt_printf("tbattery : Activation périodique\n");
        
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        
        if(status==STATUS_OK)
        {
            rt_mutex_acquire(&mutexRobotComm, TM_INFINITE);
            robot->get_vbat(robot,&level);
            rt_mutex_release(&mutexRobotComm);
            
            gestion_echec_comm(status);
            
            if(status==STATUS_OK)
            {
                d_battery_set_level(battery,level);

                message = d_new_message();
                message->put_battery_level(message, battery);

                rt_printf("tbattery : Envoi message\n");
                message->print(message, 100);

                if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) 
                {
                    message->free(message);
                }
            }
        }
    }
}

int write_in_queue(RT_QUEUE *msgQueue, void * data, int size) {
    void *msg;
    int err;

    msg = rt_queue_alloc(msgQueue, size);
    memcpy(msg, &data, size);

    if ((err = rt_queue_send(msgQueue, msg, sizeof (DMessage), Q_NORMAL)) < 0) {
        rt_printf("Error msg queue send: %s\n", strerror(-err));
    }
    rt_queue_free(&queueMsgGUI, msg);

    return err;
}

int get_threadCommuniquer() {
    int var;
    rt_mutex_acquire(&mutexTComm, TM_INFINITE);
    var = threadCommuniquer;
    rt_mutex_release(&mutexTComm);
    return var;
}
